#include <controller.h>
#include <model.h>
#include <user.h>
#include <defs.h>
#include <ts7200.h>
#include <bwio.h>
#include <track.h>

void MyTr(int train_number, int train_speed) {
  UpdateTrainSpeed(train_number, train_speed);
  Putc( COM1, train_speed );
  Putc( COM1, train_number );
}

void MyRv(int train_number) {
  int train_speed = GetTrainSpeed(train_number);
  Putc(COM1, 0);
  Putc(COM1, train_number);
  Delay(40);
  UpdateTrainReverse(train_number);
  Putc(COM1, TRAIN_REVERSE);
  Putc(COM1, train_number);
  Putc(COM1, train_speed);
  Putc(COM1, train_number);
}

void MySw(int switch_number, int switch_direction) {
  switch (switch_direction) {
  case 's':
  case 'S':
    UpdateSwitchState(switch_number, 'S');
    Putc(COM1, TRAIN_SW_STRAIGHT);
    break;
  case 'c':
  case 'C':
    UpdateSwitchState(switch_number, 'C');
    Putc(COM1, TRAIN_SW_CURVED);
    break;
  }
  Putc(COM1, switch_number);
}

void MyStop() {
  Putc( COM1, TRAIN_STOP );
}

void MyGo() {
  Putc( COM1, TRAIN_GO );
}

void MyWh() {
  Putc( COM1, TRAIN_SENSORS_BASE + NUM_SENSOR_MODULES );
}

void Controller() {
  int sender_tid;
  command cmd;
  
  // notify parent
  int parent_tid;
  Receive(&parent_tid, NULL, 0);
  Reply(parent_tid, NULL, 0);
  
  RegisterAs("train-controller");
  for (;;) {    
    Receive(&sender_tid, (char*)&cmd, sizeof(command));
    switch (cmd.type) {
      case TR:
        MyTr(cmd.arg0, cmd.arg1);
        break;
      case SW:
        MySw(cmd.arg0, cmd.arg1);
        break;
      case RV:
        MyRv(cmd.arg0);
        break;
      case WH:
        MyWh();
        break;
      case GO:
        MyGo();
        break;
      case STOP:
        MyStop();
        break;
      default:
        Panic("Controller", "Uknown command!");
        break;
    }
    Reply(sender_tid, NULL, 0);
  }
}

void Sw(int switch_number, char switch_direction) {
  int controller_tid = WhoIs("train-controller");
  command cmd;
  cmd.type = SW;
  cmd.arg0 = switch_number;
  cmd.arg1 = switch_direction;
  Send(controller_tid, (char*)&cmd, sizeof(command), NULL, 0);
}

void Stop() {
  int controller_tid = WhoIs("train-controller");
  command cmd;
  cmd.type = STOP;
  Send(controller_tid, (char*)&cmd, sizeof(command), NULL, 0);
}

void Go() {
  int controller_tid = WhoIs("train-controller");
  command cmd;
  cmd.type = GO;
  Send(controller_tid, (char*)&cmd, sizeof(command), NULL, 0);
}

void Tr(int train_number, int train_speed) {
  int controller_tid = WhoIs("train-controller");
  command cmd;
  cmd.type = TR;
  cmd.arg0 = train_number;
  cmd.arg1 = train_speed;
  Send(controller_tid, (char*)&cmd, sizeof(command), NULL, 0);
}

void Rv(int train_number) {  
  int controller_tid = WhoIs("train-controller");
  command cmd;
  cmd.type = RV;
  cmd.arg0 = train_number;
  Send(controller_tid, (char*)&cmd, sizeof(command), NULL, 0);
}

void Wh() {
  int controller_tid = WhoIs("train-controller");
  command cmd;
  cmd.type = WH;
  Send(controller_tid, (char*)&cmd, sizeof(command), NULL, 0);
}

